#include <stdio.h>
#include "unistd.h"
#include "sys/types.h"
#include "sys/stat.h"
#include "fcntl.h"
#include "stdlib.h"
#include "string.h"
#include <sys/mman.h>

#include "cam_ctrl.h"
#include "gl7004.h"
#include "dma.h"
#include "socktool.h"

extern u32 img_h;

void axi_write_reg(unsigned int base_addr, unsigned int offset_addr, unsigned int reg_value)
{
    unsigned int *reg_addr;
    int fd = open("/dev/mem", O_RDWR | O_SYNC);
    reg_addr = (unsigned int*)mmap(NULL, 65536, PROT_READ | PROT_WRITE, MAP_SHARED, fd, (off_t)base_addr);
    reg_addr[offset_addr] = reg_value;
    munmap(reg_addr, 65536);
    close(fd);
}

unsigned int axi_read_reg(unsigned int base_addr, unsigned int offset_addr)
{
    unsigned int ret;
    unsigned int *reg_addr;
    int fd = open("/dev/mem", O_RDWR | O_SYNC);
    reg_addr = (unsigned int*)mmap(NULL, 65535, PROT_READ | PROT_WRITE, MAP_SHARED, fd, (off_t)base_addr);
    ret = reg_addr[offset_addr];
    munmap(reg_addr, 65535);
    close(fd);
    return ret;
}

void sensor_reset()
{
    axi_write_reg(CAM_CTRL_BASEADDR, SENSOR_RESET, 0);
    usleep(10);
    axi_write_reg(CAM_CTRL_BASEADDR, SENSOR_RESET, 1);
}

void lvdsrx_reset()
{
    axi_write_reg(CAM_CTRL_BASEADDR, LVDSRX_RESET, 1);
    usleep(10);
    axi_write_reg(CAM_CTRL_BASEADDR, LVDSRX_RESET, 0);
}

void lvdsrx_train()
{
    axi_write_reg(CAM_CTRL_BASEADDR, LVDSRX_TRAIN, 1);
    usleep(10);
    axi_write_reg(CAM_CTRL_BASEADDR, LVDSRX_TRAIN, 0);
}

void set_trig_source(unsigned int trig_source)
{
    axi_write_reg(CAM_CTRL_BASEADDR, TRIG_SOURCE, trig_source);
}

void set_line_rate(unsigned int trig_freq_hz)
{
    axi_write_reg(CAM_CTRL_BASEADDR, TRIG_PERIOD, 100000000/trig_freq_hz - 1);
}

void set_exp_time(unsigned int exp_time_us)
{
    axi_write_reg(CAM_CTRL_BASEADDR, EXP_TIME, 100*exp_time_us);
}

void set_remote_port(unsigned int port)
{
    axi_write_reg(CAM_CTRL_BASEADDR, DES_PORT, port);
}

void set_local_ip(char* m_szLocalIp)
{
    char temp[4],ss;
    sscanf(m_szLocalIp, "%d%c%d%c%d%c%d",temp,&ss,temp+1,&ss,temp+2,&ss,temp+3);
    unsigned int src_ip = (temp[0]<<24)+(temp[1]<<16)+(temp[2]<<8)+temp[3];
    axi_write_reg(CAM_CTRL_BASEADDR, SRC_IP, src_ip);
}

void set_remote_ip_mac(char* m_szRemoteIp)
{
    char temp[4],ss;
    unsigned char mac[6];
    sscanf(m_szRemoteIp, "%d%c%d%c%d%c%d",temp,&ss,temp+1,&ss,temp+2,&ss,temp+3);
    unsigned int des_ip = (temp[0]<<24)+(temp[1]<<16)+(temp[2]<<8)+temp[3];
    axi_write_reg(CAM_CTRL_BASEADDR, DES_IP, des_ip);
    net_eth_dst_get_mac("eth0", temp, mac);
    unsigned int mac_h = (mac[0]<<16)+(mac[1]<<8)+mac[2];
    unsigned int mac_l = (mac[3]<<16)+(mac[4]<<8)+mac[5];

    axi_write_reg(CAM_CTRL_BASEADDR, DES_MAC_H, mac_h);
    axi_write_reg(CAM_CTRL_BASEADDR, DES_MAC_L, mac_l);
}

void set_img_h(unsigned int img_h)
{
    axi_write_reg(CAM_CTRL_BASEADDR, SIZE_Y, img_h);
}

void set_pkt(unsigned int size_pkt, u32 img_h)
{
    unsigned int size_payload = size_pkt - 36;
    unsigned int num_pkt, size_pkt_last;
    if((4096 * img_h) % size_payload == 0){
        num_pkt = 4096 * img_h/size_payload +1;
        size_pkt_last = size_pkt;
    }
    else {
        num_pkt = 4096 * img_h/size_payload + 2;
        size_pkt_last = 4096 * img_h - (num_pkt -2)*size_payload + 36;
    }
    axi_write_reg(CAM_CTRL_BASEADDR, NUM_PKT, num_pkt);
    axi_write_reg(CAM_CTRL_BASEADDR, SIZE_PKT, size_pkt);//8164
    axi_write_reg(CAM_CTRL_BASEADDR, SIZE_PKT_LAST, size_pkt_last);
    printf("num_pkt: %d size_pkt: %d size_pkt_last: %d\n",num_pkt, size_pkt, size_pkt_last);
}

void set_gain(unsigned int gain)
{
    axi_write_reg(CAM_CTRL_BASEADDR, GAIN, gain*256/10);
}

void set_offset(unsigned int offset)
{
    axi_write_reg(CAM_CTRL_BASEADDR, OFFSET, (offset+32)<<4);
}

void fpn_reset()
{
    for(int i=0; i<=2047; i++){
        axi_write_reg(FPN_COE_BASEADDR, i, 0);
    }
}

void prnu_reset()
{
    for(int i=0; i<=2047; i++){
        axi_write_reg(PRNU_COE_BASEADDR, i, 1024 + (1024 << 16));
    }
}

unsigned int px_value[4096];
void px_read()
{
    for(int i=0; i<=4095; i++) px_value[i] = 0;
    for(int j=0;j<=49;j++){
        for(int i=0; i<=2047; i++){
            unsigned int temp = axi_read_reg(MES_DATA_BASEADDR, i);
            px_value[2*i] += temp & 0xffff;
            px_value[2*i+1] += (temp>>16) & 0xffff;
        }
    }
    for(int i=0; i<=4095; i++) {
        px_value[i] = (px_value[i]/50);
    }
}

unsigned int fpn_coe[4096];
void fpn_set()
{
    px_read();
    for(int i=0; i<=2047; i++){
        fpn_coe[2*i] = px_value[2*i];
        fpn_coe[2*i+1] = px_value[2*i+1];
        axi_write_reg(FPN_COE_BASEADDR, i, fpn_coe[2*i] + (fpn_coe[2*i+1] << 16));
    }
    FILE* fp = fopen("fpn.coe","w");
    for(int i=0;i<=4095;i++) fprintf(fp,"%d \n", fpn_coe[i]);
    fclose(fp);
}
unsigned int prnu_coe[4096];
void prnu_set()
{
    unsigned int target = 0;
    px_read();
    for(int i=0; i<=2047; i++){
        target = target + px_value[2*i] + px_value[2*i+1];
    }
    target = target/4096;

    for(int i=0; i<=2047; i++){
        if(px_value[2*i] - fpn_coe[2*i] != 0)
        prnu_coe[2*i] = target*1024/(px_value[2*i] - fpn_coe[2*i]);
        if(px_value[2*i+1] - fpn_coe[2*i+1] != 0)
        prnu_coe[2*i+1] = target*1024/(px_value[2*i+1] - fpn_coe[2*i+1]);
        axi_write_reg(PRNU_COE_BASEADDR, i, prnu_coe[2*i] + (prnu_coe[2*i+1] << 16));
    }
    FILE* fp = fopen("prnu.coe","w");
    for(int i=0;i<=4095;i++) fprintf(fp,"%d \n", prnu_coe[i]);
    fclose(fp);
}

void prnu_set_target(unsigned int target)
{
    target = target << 4;
    px_read();
    for(int i=0; i<=2047; i++){
        if(px_value[2*i] - fpn_coe[2*i] != 0)
        prnu_coe[2*i] = target*1024/(px_value[2*i] - fpn_coe[2*i]);
        if(px_value[2*i+1] - fpn_coe[2*i+1] != 0)
        prnu_coe[2*i+1] = target*1024/(px_value[2*i+1] - fpn_coe[2*i+1]);
        axi_write_reg(PRNU_COE_BASEADDR, i, prnu_coe[2*i] + (prnu_coe[2*i+1] << 16));
    }
}

void fpn_load()
{
    FILE* fp = fopen("fpn.coe","r");
    if (fp == NULL) return;
    for(int i=0;i<=4095;i++) fscanf(fp,"%d\n", fpn_coe+i);
    fclose(fp);
    for(int i=0; i<=2047; i++){
        axi_write_reg(FPN_COE_BASEADDR, i, fpn_coe[2*i] + (fpn_coe[2*i+1] << 16));
    }
}

void prnu_load()
{
    FILE* fp = fopen("prnu.coe","r");
    if (fp == NULL) return;
    for(int i=0;i<=4095;i++) fscanf(fp,"%d\n", prnu_coe+i);
    fclose(fp);
    for(int i=0; i<=2047; i++){
        axi_write_reg(PRNU_COE_BASEADDR, i, prnu_coe[2*i] + (prnu_coe[2*i+1] << 16));
    }
}

SG_Descriptor desc[3];
unsigned char* sg_virt_addr;

void mmap_init(){
    sg_virt_addr = (unsigned char*)mmap(NULL, 4096*10000, PROT_READ | PROT_WRITE, MAP_SHARED, open("/dev/mem", O_RDWR | O_SYNC), (off_t)(0x10000000));
}

void dma_init(unsigned int  img_h)
{
    desc[0].buffer_addr = 0x10001000;
    desc[0].control = 4096*img_h+(1<<27)+(1<<26);
    desc[0].nextdesc = 0x10000040;
    ////////////////////////////////////////////
    desc[1].buffer_addr = 0x10001000+4096*img_h;
    desc[1].control = 4096*img_h+(1<<26)+(1<<27);
    desc[1].nextdesc = 0x10000000;
    ////////////////////////////////////////////
    desc[2].buffer_addr = 0x10001000;
    desc[2].control = 4096*img_h+(1<<26)+(1<<27);
    desc[2].nextdesc = 0x10000080;
    ////////////////////////////////////////////
    memcpy(sg_virt_addr, desc, 0x34);
    memcpy(sg_virt_addr + 0x40, desc + 1, 0x34);
    memcpy(sg_virt_addr + 0x80, desc + 2, 0x34);

    axi_write_reg(DMA_BASE_ADDR, S2MM_DMACR >> 2, 0x04);
    axi_write_reg(DMA_BASE_ADDR, S2MM_CURDESC >> 2, 0x10000000);
    axi_write_reg(DMA_BASE_ADDR, S2MM_DMACR >> 2, 0x1011);
    axi_write_reg(DMA_BASE_ADDR, S2MM_TAILDESC >> 2, 0x100000C0);

    usleep(100);
}

void gvsp_send_reset()
{
    axi_write_reg(CAM_CTRL_BASEADDR, GVSP_RST, 1);
    axi_write_reg(CAM_CTRL_BASEADDR, GVSP_RST, 0);
}

void fifo_reset()
{
    axi_write_reg(CAM_CTRL_BASEADDR, FIFO_RST, 1);
    axi_write_reg(CAM_CTRL_BASEADDR, FIFO_RST, 0);
}

void set_local_mac()
{
    unsigned char mac[6];
    net_eth_get_mac("eth0", mac);
    unsigned int mac_h = (mac[0]<<16)+(mac[1]<<8)+mac[2];
    unsigned int mac_l = (mac[3]<<16)+(mac[4]<<8)+mac[5];
    axi_write_reg(CAM_CTRL_BASEADDR, SRC_MAC_H, mac_h);
    axi_write_reg(CAM_CTRL_BASEADDR, SRC_MAC_L, mac_l);
}

void set_trig_act(unsigned int act)
{
    axi_write_reg(CAM_CTRL_BASEADDR, TRIG_ACT, act);
}

void set_enc_mod(unsigned int mod)
{
    axi_write_reg(CAM_CTRL_BASEADDR, TRIG_ENC, mod);
}

void led_red_off()
{
    axi_write_reg(CAM_CTRL_BASEADDR, LED_RED, 0);
}

void set_tdi(unsigned int line)
{
    axi_write_reg(CAM_CTRL_BASEADDR, TDI_LINE, line);
}

void set_x_rev(unsigned int flip_h)
{
    axi_write_reg(CAM_CTRL_BASEADDR, FLIP_H, flip_h);
}

void cam_init()
{
    gvsp_send_reset();
    set_trig_source(TRIG_OFF);
    sensor_reset();
    usleep(1000);
    lvdsrx_reset();
    gl7004_init();
    lvdsrx_train();
    fifo_reset();
    set_trig_source(TRIG_SRC_INT);
    set_exp_time(10);
    set_line_rate(10000);
    set_gain(10);
    set_offset(0);
    set_tdi(0);
    set_x_rev(0);
    axi_write_reg(CAM_CTRL_BASEADDR, STREAM_EN, 1);
    mmap_init();
    fpn_load();
    prnu_load();
    led_red_off();
    printf("Init finish\n");
}
